// --------------------------------------------------------------------------------------------------------------------
// <copyright file="JointController.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation.JointController
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Manipulation;

    /// <summary>
    /// Interface for joint controllers.
    /// </summary>
    public abstract class JointController
    {
        /// <summary>
        /// The current reference values
        /// </summary>
        private double[] references;

        /// <summary>
        /// Maximum absolute value of output
        /// </summary>
        private double[] maxAbsOutput;

        /// <summary>
        /// The number of joints under control
        /// </summary>
        private int numJoints;

        /// <summary>
        /// Initializes a new instance of the <see cref="JointController"/> class.
        /// </summary>
        /// <param name="numJoints">Number of joints to control.</param>
        public JointController(int numJoints)
        {
            this.ReferenceLockObject = new object();
            this.numJoints = numJoints;
            this.references = new double[numJoints];
            this.ControlOutput = new double[numJoints];
            this.ControllerDeactivated = false;
            this.maxAbsOutput = new double[numJoints];
        }

        /// <summary>
        /// Gets a copy of controller reference values
        /// </summary>
        public double[] References
        {
            get
            {
                double[] referenceReturnArray = new double[this.numJoints];
                lock (this.ReferenceLockObject)
                {
                    Array.Copy(this.references, referenceReturnArray, this.numJoints);
                }

                return referenceReturnArray;
            }
        }

        /// <summary>
        /// Gets a copy of the maximum absolute output values
        /// </summary>
        public double[] MaxAbsOutput
        {
            get
            {
                double[] maxAbsOutputReturnArray = new double[this.numJoints];
                lock (this.ReferenceLockObject)
                {
                    Array.Copy(this.maxAbsOutput, maxAbsOutputReturnArray, this.numJoints);
                }

                return maxAbsOutputReturnArray;
            }
        }

        /// <summary>
        /// Gets a value indicating whether or not controller has been deactivated.
        /// </summary>
        public bool IsControllerDeactivated
        {
            get
            {
                return this.ControllerDeactivated;
            }
        }

        /// <summary>
        /// Gets the number of joints controller is configured for.
        /// </summary>
        public int NumJoints
        {
            get
            {
                return this.numJoints;
            }
        }

        /// <summary>
        /// Gets or sets the signal type for references (e.g. velocity, position, torque, etc...)
        /// </summary>
        public JointSignalType ReferenceSignalType { get; protected set; }

        /// <summary>
        /// Gets or sets the signal type for controller output (e.g. velocity, position, torque, etc...)
        /// </summary>
        public JointSignalType ControlOutputSignalType { get; protected set; }

        /// <summary>
        /// Gets or sets the current output values
        /// </summary>
        protected double[] ControlOutput { get; set; }

        /// <summary>
        /// Gets or sets the object for locking access to references
        /// </summary>
        protected object ReferenceLockObject { get; set; }

        /// <summary>
        /// Gets or sets a value indicating whether or not controller has been deactivated
        /// </summary>
        protected bool ControllerDeactivated { get; set; }

        /// <summary>
        /// Gets reference to "references" array without making a copy.  Values may change
        /// unless lock is taken on <c>ReferenceLockObject</c>.
        /// </summary>
        protected double[] ReferenceNoCopy
        {
            get
            {
                return this.references;
            }
        }

        /// <summary>
        /// Gets reference to "maxAbsOutput" array without making a copy.  Values may change
        /// unless lock is taken on <c>ReferenceLockObject</c>.
        /// </summary>
        protected double[] MaxAbsOutputNoCopy
        {
            get
            {
                return this.maxAbsOutput;
            }
        }

        /// <summary>
        /// Execute one cycle of the controller
        /// </summary>
        /// <param name="status">Current status of the manipulator to extract joint information from.</param>
        /// <returns>Current controller output.</returns>
        public abstract double[] Cycle(ManipulatorStatus status);

        /// <summary>
        /// Set the values of the control reference
        /// </summary>
        /// <param name="references">Reference values.</param>
        public void SetReferences(double[] references)
        {
            if (this.ControllerDeactivated)
            {
                throw new ControllerDeactivatedException();
            }

            lock (this.ReferenceLockObject)
            {
                Array.Copy(references, this.references, this.numJoints);
            }
        }

        /// <summary>
        /// Set the values of maximum absolute output.
        /// </summary>
        /// <param name="maxAbsOutput">Maximum absolute output values.</param>
        public void SetMaxAbsOutput(double[] maxAbsOutput)
        {
            if (this.ControllerDeactivated)
            {
                throw new ControllerDeactivatedException();
            }

            lock (this.ReferenceLockObject)
            {
                Array.Copy(maxAbsOutput, this.maxAbsOutput, this.numJoints);
            }
        }

        /// <summary>
        /// Deactivate the controller.  When called by execution component, indicates that controller is no longer being 
        /// used in control loop due to replacement or other condition.  When called by higher level control component
        /// indicates that the control loop show cease to use this controller to generate actions.
        /// </summary>
        public void DeactivateController()
        {
            this.ControllerDeactivated = true;
        }
    }
}
